﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using AI.Fuzzy.Library;
using Logging;

namespace SGGWElevator
{
    public class ElevatorEngine
    {
        SugenoFuzzySystem _fsCruiseControl = null;
        private double _velocity;
        private double _position;

        public ElevatorEngine(double velocity, double position)
        {
            this._velocity = velocity;
            this._position = position;


            SugenoFuzzySystem fsCruiseControl = new SugenoFuzzySystem();

            //
            // TUTAJ NALEŻY WYREGULOWAC WINDĘ
            //
            FuzzyVariable fvRoznicaPozycji = new FuzzyVariable("RoznicaPozycji", -400, 400);
            fvRoznicaPozycji.Terms.Add(new FuzzyTerm("slower", new TriangularMembershipFunction(-400.0, -390.0, -50.0)));
            fvRoznicaPozycji.Terms.Add(new FuzzyTerm("zero", new TriangularMembershipFunction(-20.0, -0.0, 20.0)));
            fvRoznicaPozycji.Terms.Add(new FuzzyTerm("faster", new TriangularMembershipFunction(50.0, 390.0, 400.0)));
            fsCruiseControl.Input.Add(fvRoznicaPozycji);


            //TUTAJ REGULACJA PREDKOSCI, JAKIE WINDA MOZE OSIAGNAC I ICH ZMIANY
            FuzzyVariable fvRoznicaPredkosci = new FuzzyVariable("RoznicaPredkosci", -5.0, 5.0);
            fvRoznicaPredkosci.Terms.Add(new FuzzyTerm("slower", new TriangularMembershipFunction(-5.0, -2.0, -1.0)));
            fvRoznicaPredkosci.Terms.Add(new FuzzyTerm("zero", new TriangularMembershipFunction(-4.0, -0.0, 4.0)));
            fvRoznicaPredkosci.Terms.Add(new FuzzyTerm("faster", new TriangularMembershipFunction(1.0, 2.0, 5.0)));
            fsCruiseControl.Input.Add(fvRoznicaPredkosci);

            //
            // Create output variables for the system
            //
            SugenoVariable svAccelerate = new SugenoVariable("Accelerate");
            svAccelerate.Functions.Add(fsCruiseControl.CreateSugenoFunction("zero", new double[] { 0.0, 0.0, 0.0 }));
            svAccelerate.Functions.Add(fsCruiseControl.CreateSugenoFunction("faster", new double[] { 0.0, 0.0, 0.5 }));
            svAccelerate.Functions.Add(fsCruiseControl.CreateSugenoFunction("slower", new double[] { 0.0, 0.0, -0.5 }));
            svAccelerate.Functions.Add(fsCruiseControl.CreateSugenoFunction("func", new double[] { -0.04, -0.4, 0.0 }));
            fsCruiseControl.Output.Add(svAccelerate);

            //
            // Create fuzzy rules
            //
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "slower", "slower", "faster");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "slower", "zero", "faster");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "slower", "faster", "zero");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "zero", "slower", "faster");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "zero", "zero", "func");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "zero", "faster", "slower");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "faster", "slower", "zero");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "faster", "zero", "slower");
            AddSugenoFuzzyRule(fsCruiseControl, fvRoznicaPozycji, fvRoznicaPredkosci, svAccelerate, "faster", "faster", "slower");

            //
            // Adding the same rules in text form
            //
            ///////////////////////////////////////////////////////////////////
            //SugenoFuzzyRule rule1 = fsCruiseControl.ParseRule("if (SpeedError is slower) and (SpeedErrorDot is slower) then (Accelerate is faster)");
            //SugenoFuzzyRule rule2 = fsCruiseControl.ParseRule("if (SpeedError is slower) and (SpeedErrorDot is zero) then (Accelerate is faster)");
            //SugenoFuzzyRule rule3 = fsCruiseControl.ParseRule("if (SpeedError is slower) and (SpeedErrorDot is faster) then (Accelerate is zero)");
            //SugenoFuzzyRule rule4 = fsCruiseControl.ParseRule("if (SpeedError is zero) and (SpeedErrorDot is slower) then (Accelerate is faster)");
            //SugenoFuzzyRule rule5 = fsCruiseControl.ParseRule("if (SpeedError is zero) and (SpeedErrorDot is zero) then (Accelerate is func)");
            //SugenoFuzzyRule rule6 = fsCruiseControl.ParseRule("if (SpeedError is zero) and (SpeedErrorDot is faster) then (Accelerate is slower)");
            //SugenoFuzzyRule rule7 = fsCruiseControl.ParseRule("if (SpeedError is faster) and (SpeedErrorDot is slower) then (Accelerate is faster)");
            //SugenoFuzzyRule rule8 = fsCruiseControl.ParseRule("if (SpeedError is faster) and (SpeedErrorDot is zero) then (Accelerate is slower)");
            //SugenoFuzzyRule rule9 = fsCruiseControl.ParseRule("if (SpeedError is faster) and (SpeedErrorDot is faster) then (Accelerate is slower)");

            //fsCruiseControl.Rules.Add(rule1);
            //fsCruiseControl.Rules.Add(rule2);
            //fsCruiseControl.Rules.Add(rule3);
            //fsCruiseControl.Rules.Add(rule4);
            //fsCruiseControl.Rules.Add(rule5);
            //fsCruiseControl.Rules.Add(rule6);
            //fsCruiseControl.Rules.Add(rule7);
            //fsCruiseControl.Rules.Add(rule8);
            //fsCruiseControl.Rules.Add(rule9);

            ///////////////////////////////////////////////////////////////////
            this._fsCruiseControl = fsCruiseControl;
        }

        private void AddSugenoFuzzyRule(
            SugenoFuzzySystem fs,
            FuzzyVariable fv1,
            FuzzyVariable fv2,
            SugenoVariable sv,
            string value1,
            string value2,
            string result
            )
        {
            SugenoFuzzyRule rule = fs.EmptyRule();
            rule.Condition.Op = OperatorType.And;
            rule.Condition.ConditionsList.Add(rule.CreateCondition(fv1, fv1.GetTermByName(value1)));
            rule.Condition.ConditionsList.Add(rule.CreateCondition(fv2, fv2.GetTermByName(value2)));
            rule.Conclusion.Var = sv;
            rule.Conclusion.Term = sv.GetFuncByName(result);
            fs.Rules.Add(rule);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="floor"></param>
        public void GoToFloor(double floor)
        {
            double roznica_pozycji = _position - floor; //jestem na 0, chcę jechać na 400, czyli wynik to -400 bo o 400 za nisko

            //
            // Get variables from the system (for convinience only)
            //
            FuzzyVariable fvRoznicaPozycji = _fsCruiseControl.InputByName("RoznicaPozycji");
            FuzzyVariable fvRoznicaPredkosci = _fsCruiseControl.InputByName("RoznicaPredkosci");
            SugenoVariable svAccelerate = _fsCruiseControl.OutputByName("Accelerate");

            //
            // Fuzzify input values
            //
            Dictionary<FuzzyVariable, double> inputValues = new Dictionary<FuzzyVariable, double>();
            inputValues.Add(fvRoznicaPozycji, (double)roznica_pozycji);
            inputValues.Add(fvRoznicaPredkosci, (double)_velocity);

            //
            // Calculate the result
            //
            Dictionary<SugenoVariable, double> result = _fsCruiseControl.Calculate(inputValues);

            //tbAccelerate.Text = (result[svAccelerate] * 100.0).ToString("f1");
            //Console.WriteLine(result[svAccelerate]);
            _velocity += result[svAccelerate];
            _position += _velocity;
        }

        /// <summary>
        /// Aktualna pozycja windy
        /// </summary>
        public double ActualPosition
        {
            get
            {
                return _position;
            }
        }

        /// <summary>
        /// Aktualna prędkość windy
        /// </summary>
        public double ActualVelocity
        {
            get
            {
                return _velocity;
            }
        }

        public static double GetFloorPosition(Floor floor)
        {
            return (double)floor * FLOOR_HEIGHT;
        }

        private readonly static double FLOOR_HEIGHT = 85;
    }
}
